#ifndef ANGLESOLVER_H
#define ANGLESOLVER_H

#pragma once
#include "opencv2/objdetect.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/videoio.hpp"
#include <iostream>
#include <cmath>
#include "armor_recog.hpp"

using namespace cv;
using namespace std;

struct Vector3d
{
    // 侧视图法线方向（向右）
    double x;
    // 正视图法线方向
    double y;
    // 俯视图法线方向
    double z;
};

class angleSolver
{
private:
    Vector3d enemy_pos;   // 敌方装甲板相对炮管位置
    Point3f enemy_v = Point3f(0,0,0);     // 敌方装甲板相对炮管速度 单位m/s
    double bullet_v = 20; // 子弹速度  单位m/s
    double g = 10;        //9.7883; //重力加速度 单位kg*m/s^2

    double yaw = 0;
    double pitch = 0;

    double solveQuadratic(double a, double b, double c, char flag); // 解二次方程，无法解时返回INFINITY
    double calculatePitch(double distance);
    double calculateYaw();
    // void setEnemyPos(Vector3d pos);

    int img_width = 752;         // 图像宽度
    int img_height = 480;        // 图像高度
    double focal_length = 645.4; // 焦距
    double bar_length = 55;      // 灯条实际长度（毫米制）

    // 以下可以忽略
    double H = 0.5;       //炮台离地高度
    double L = 0.01;      //枪管长度
    double Co = 0.000001; //

    double Miu = 0.00001; //
    double Z = 0.5;
    double V = 8; //initial speed of the bullet
    double X = 0;
    double Y = 5;
    double X_velocity = 0; // to the right
    double Y_velocity = 0; // to the front
    double select_alpha(double dis);
    double get_thet(double X, double Y, double X_velocity, double Y_velocity, double thet0);
    double dis = sqrt(X * X + Y * Y + Z * Z); //??
    double get_para_t(double vx0, double dis, double alpha);
    double get_para_height(double vx0, double vy0, double dis, double alpha);
    double get_str_v(double alpha);
    double get_height(double alpha, double dis);

public:
    angleSolver();
    double getPitch(); //返回pitch弧度值
    double getYaw();   //返回yaw弧度值
    int solve();       //计算yaw和pitch，若无法射中则返回-1
    void test();
    void setEnemyPos(int target_x, int target_y, int img_bar_length, double currentPitch, double currentYaw);
    void setEnemyVelocity(Point3f velocity);
    void setBulletSpeed(double bullet_speed);
    void init(int img_width, int img_height, double focal_length, double bar_length);

    // 以下暂时忽略
    void get_Position(Armor &armor);
    double get_ALPHA();
    double get_THET();
    void setXYZDis(double X, double Y, double Z);
    void setRelativeV(double Vx, double Vy);
    double getDis();
    double setDis();
};

#endif